import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from robot_control_interface.action import Moverobot

class Action_controller(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info('This a controller node:{}'.format(name))
        self.action_client = ActionClient(self,Moverobot,'move_robot')
        self.send_goal_timer = self.create_timer(1,self.send_goal)

    def send_goal(self):
        self.send_goal_timer.cancel()
        goal_msg = Moverobot.Goal()
        goal_msg.distance = 5.0
        
        self.get_logger().warn('waiting for robot server')
        self.action_client.wait_for_server()

        self.get_logger().warn(f'sending goal:{goal_msg.distance}')
        self.send_goal_future = self.action_client.send_goal_async(goal_msg,self.feedback_callback)
        self.send_goal_future.add_done_callback(self.goal_response_callback)

    def feedback_callback(self,feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(f'receive feedback: {feedback.pose}')

    def goal_response_callback(self,future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info(f'goal rejected:(')
            return
        self.get_logger().info('goal accepted:)')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f'Result: {result.pose}')

def main(args=None):
    rclpy.init(args=args)
    node = Action_controller('action_c')
    rclpy.spin(node)
    rclpy.shutdown()
    